// MapAndSearch.cpp - MapAndSearch class
// Implements the A* pathfinding algorithm.
// Be sure to call parseNodeFiles() first to create the NodeMap.

#include "MapAndSearch.h"
#include <math.h>
#include <iostream>
#include <stack>

MapAndSearch::MapAndSearch(void)
{
}

MapAndSearch::~MapAndSearch(void)
{
	for (NodeMap::iterator itr = nodeMapPtr->begin(); itr != nodeMapPtr->end(); ++itr)
	{
		delete itr->second;
	}
	delete nodeMapPtr;
}

void MapAndSearch::initializeMap(std::string locsFile, std::string connsFile)
{
	std::string start = getStartFromUser();
	std::string destination = getDestinationFromUser();
	std::string excludedPoint = getExcludedPointFromUser();

	if(excludedPoint.size() != 0)
	{
		if(excludedPoint == start || excludedPoint == destination)
		{
			std::cout << "\nThis is the start/end node, cannot exclude. Continuing without exclusion...\n";
			excludedPoint.clear();
		}
	}

	nodeMapPtr = Parser::parseData(locsFile, connsFile, excludedPoint);

	NodeMap::iterator itr = nodeMapPtr->find(start);
	if (itr == nodeMapPtr->end())
	{
		std::cout << "Starting node is not valid." << std::endl;
		exit(1);
	}
	startNodePtr = itr->second;

	itr = nodeMapPtr->find(destination);
	if (itr == nodeMapPtr->end())
	{
		std::cout << "Destination node is not valid." << std::endl;
		exit(1);
	}
	destinationNodePtr = itr->second;
}

// Prompt and return starting Node*
std::string MapAndSearch::getStartFromUser(void)
{
	std::cout << "Please enter the starting node: ";
	std::string starting;
	do
	{
		std::getline(std::cin, starting);
	}
	while (starting.length() == 0);
	return starting;

}

// Prompt and return destination Node*
std::string MapAndSearch::getDestinationFromUser(void)
{
	std::cout << "Please enter the destination node: ";
	std::string destiny;
	do
	{
		std::getline(std::cin, destiny);
	}
	while (destiny.length() == 0);
	return destiny;
}

// Prompt and return an (optional) Node* to exclude
std::string MapAndSearch::getExcludedPointFromUser(void)
{
	std::cout << "If you would like to exclude a node please enter that node: ";
	std::string exclude;
	std::getline(std::cin, exclude);
	return exclude;
}

void MapAndSearch::printNodes(void)
{
	for (NodeMap::iterator itr = nodeMapPtr->begin(); itr != nodeMapPtr->end(); ++itr)
	{
		itr->second->print();
		std::cout << std::endl;
	}
	std::cout << "\n" << "Number of nodes: " << nodeMapPtr->size() << std::endl;
}

// Returns the sum of the distance of the current path from the start to a Node (int g)
// and the straight line distance from a Node to the destination (int h, our heuristic).
int MapAndSearch::calculateFValue(const Node &currentNode) const
{
	Node tmpNode = currentNode;
	int g = 0;
	for (Node *tmpNodePtr = &tmpNode; tmpNodePtr->getParent() != NULL; tmpNodePtr = tmpNodePtr->getParent())
	{
		g += calculateEuclidianDistance(*tmpNodePtr, *(tmpNodePtr->getParent()));
	}
	int h = calculateEuclidianDistance(tmpNode, *destinationNodePtr);
	return g + h;
}

// Returns straight line distance between two Nodes.
int MapAndSearch::calculateEuclidianDistance(const Node &a, const Node &b) const
{
	double radicand = pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2);
	return (int)sqrt(radicand);
}

bool MapAndSearch::newAStar()
{
	startNodePtr->setF(0);
	startNodePtr->setParent(NULL);
	openList.push(startNodePtr);

	while (!openList.empty())
	{
		// Get Node on openList with lowest F
		Node *currentNodePtr = openList.top();
		openList.pop();
		if (currentNodePtr == destinationNodePtr)
		{
			//The node popped off the openList is our destination.
			//We have completed A*
			return true;
		}

		// For every successor of the current node
		std::vector<Node*> successors = currentNodePtr->getConnections();
		for (std::vector<Node*>::iterator successorsItr = successors.begin();
			successorsItr != successors.end(); ++successorsItr)
		{
			if (currentNodePtr == *successorsItr)
			{
				// This successor is the parent
				continue;
			}
			// tmpSuccessor is a copy of the current successor under investigation.
			// Making a copy allows us to calculate the F of that successor without
			// modifying the instance of the successor in memory
			Node tmpSuccessor = **successorsItr;
			tmpSuccessor.setParent(currentNodePtr);
			tmpSuccessor.setF(calculateFValue(tmpSuccessor));

			// Successor of current node is already on open list
			if (tmpSuccessor.inOpenList() /* || tmpSuccessor.inClosedList() */)
			{
				// New path to successor has a lower F than the old path
				if (tmpSuccessor.getF() < (*successorsItr)->getF())
				{
					// To maintain a valid heap structure (because heap is sorted on F value),
					// remove this Node* from the openList, update F/parent, and push back onto openList
					std::stack<Node*> poppedNodes;
					while (!openList.empty())
					{
						Node *tmpNodePtr = openList.top();
						openList.pop();
						if (tmpNodePtr == *successorsItr)
						{
							tmpNodePtr->setParent(currentNodePtr);
							tmpNodePtr->setF(tmpSuccessor.getF());
							openList.push(tmpNodePtr);
							break;
						}
						poppedNodes.push(tmpNodePtr);
					}
					while (!poppedNodes.empty())
					{
						openList.push(poppedNodes.top());
						poppedNodes.pop();
					}
				}
			}
			// Successor of current node is already on closed list
			else if (tmpSuccessor.inClosedList())
			{
				// New path to successor has a lower F than the old path
				if (tmpSuccessor.getF() < (*successorsItr)->getF())
				{
					(*successorsItr)->setParent(currentNodePtr);
					(*successorsItr)->setF(tmpSuccessor.getF());
				}
			}
			// Successor of current node is unvisited
			else
			{
				(*successorsItr)->setParent(currentNodePtr);
				(*successorsItr)->setF(tmpSuccessor.getF());
				(*successorsItr)->inOpenList(true);
				openList.push(*successorsItr);
			}
		}
		// After investigating every successor of currentNodePtr, move from open to closed list
		currentNodePtr->inOpenList(false);
		currentNodePtr->inClosedList(true);
	}

	return false;
}

void MapAndSearch::printPathHome(void) const
{
	std::cout << "\nThe path from start node to end node is:" << std::endl;
	std::stack<std::string> pathHome;

	for (Node *tmpNodePtr = destinationNodePtr; tmpNodePtr != NULL; tmpNodePtr = tmpNodePtr->getParent())
	{
		pathHome.push(tmpNodePtr->getName());
	}
	while (!pathHome.empty())
	{
		std::cout << pathHome.top();
		pathHome.pop();
		if (pathHome.size() > 0)
		{
			std::cout << " -> ";
		}
	}
	std::cout << std::endl;
}

void MapAndSearch::runAStar(void)
{
	bool status = newAStar();
	if (status)
	{
		printPathHome();
	}
	else
	{
		std::cout<< "\nPath from " << startNodePtr->getName() << " to " << destinationNodePtr->getName() <<
			" does not exist." << std::endl;
	}
}
